#include "Coord.h"
#include <math.h>

#define STR_SZ 50

#define DEG2RAD 0.0174532925
#define RAD2DEG 57.2957795

Coord::Coord(){lat=lon=0.0;}
Coord::Coord(double dlat, double dlon)
{
	maked(dlat,dlon);
}
void Coord::maked(double lat,double lon)
{
	maker(lat*DEG2RAD,lon*DEG2RAD);
}
void Coord::maker(double lat,double lon)
{
	this->lat = lat;
	this->lon = lon;
}
double Coord::latd() const{return lat*RAD2DEG;}
double Coord::lond() const{return lon*RAD2DEG;}
double Coord::latr() const{return lat;}
double Coord::lonr() const{return lon;}
double Coord::dist(const Coord &c, double unit) const
{
	double dLat = c.latr()-latr();
	double dLon = c.lonr()-lonr();
	
	double a = (sin(dLat/2.0) * sin(dLat/2.0)) +
	(cos(latr()) * cos(c.latr()) * sin(dLon/2.0) * sin(dLon/2.0));
	return 2.0 * atan2(sqrt(a), sqrt(1.0-a)) * unit;
}
double Coord::bearing(const Coord &c) const
{
	double bearing;
	bearing = atan2(sin(c.lonr()-lonr())*cos(c.latr()), cos(latr())*sin(c.latr()) - sin(latr())*cos(c.latr())*cos(c.lonr()-lonr()))*RAD2DEG;
	bearing = fmod(bearing,360.0);
	if (bearing < 0){bearing+=360;}
	return bearing;
}
bool Coord::dir(const Coord &c, double dir) const
{
	return turnTo(bearing(c),dir);
}
double Coord::angle(const Coord &c, double dir) const
{
	return angleTo(bearing(c),dir);
}
bool Coord::turnTo(double from,double to)
{
	return angleTo(from,to)>0?RIGHT:LEFT;
}
double Coord::angleTo(double from,double to)
{
	double x = fmod(to-from,360.0);
	if (x > 180){x-=360;}
    else if (x <= -180){x+=360;}
    return x;
}